// (C) Copyright 2009, 2010 Communications Programming, Inc.
/***************************************************************************************************************
*  Name: vexMotor.c
*
*  Description: Functions to control a Vex motor using the OC (output compare) device for PWM
*
*  Calling Parameters:
*  
*	INT16 VexMotorOpen(	int		ocChan,		// Output Compare Channel #
*						int		timerNum );	// timer number (2 or 3)
*	
*	opens the outcompare port controlling the Vex motor.  A motor stop write will also be issued.
*
*	Returns > 0 handle to use in future i/o calls, 
*				-1 if invalid OC #(must be 1-5), timer # (must be 2 or 3), or mem alloc failed
*
*   INT16 VexMotorWrite( int handle, INT16 speed );	
*
*	speed determines the motor speed, using the range -127 (full reverse) to +127 (full forward).  
* 
*   INT16 VexServerMotorWrite( int handle, INT16 speed );	
*
*	speed determines the motor speed, using the range -127 (one-side extreme) to +127 (opposite-side extreme).  
*	Note: Vex servos span approx 120 degrees.
* 
*	Returns 0 for success, or non-0 error codes in pwm.h if failed
*
*	INT16 VexMotorClose( UINT16 handle );
*
*	Closes the Vex Motor device
*
*	Returns 0 for success, or non-0 error codes in pwm.h if failed
*
*  Revision History:
*  Date    Who  Comments
*  ------  ---  -----------------------------------------------------------------------------------------
*  101105  RLP	Created.
*  110130  RLP	Add VexServerMotorWrite()
*
***************************************************************************************************************/

#include <stdlib.h>
#include <plib.h>
#include <userLib.h>
#include <pwm\pwm.h>
#include <pwm\vexMotor.h>

// this table is indexed by the 4 MSBs of the input speed to create a linear speed

static INT16 linearSpeedTbl[] = {0,13,17,19,21,23,25,28,30,33,36,42,48,56,68,82,96};

INT16 VexMotorOpen(	int		ocNum,		// Output Compare Channel #
					int		timerNum )	// timer number (2 or 3)
{
	INT16		handle;						// handle retn'd by PWMOpen()
	UINT16		timerMult;					// timer multiplier

	// validation: oc number must be 1-5, timer 2-3
	if( ocNum < 1 || ocNum > 5 || timerNum < 2 || timerNum > 3 )
		return(-1);

	if( timerNum == 2 )
		timerMult = T2_PS_1_32;
	else 
		timerMult = T3_PS_1_32;
	
	// open the PWM port controlling the motor

	handle = PWMOpen( ocNum, timerNum, timerMult, 0xC000 );		// PWM cycle = 20ms per Vex reqmt

	if( handle == -1 )
		return(handle);				// quit now if failed

	// stop motor initially

	VexMotorWrite( handle, 0 );
	
	return( handle );				// return i/o handle to caller
}

INT16 VexMotorWrite(INT16 handle, INT16 speed)
{
	double	dcVal;					// duty cycle value to output
	INT16	rc;						// retn code
	INT16	sign;					// sign of input
	INT16	lsb;					// 3 LSBs of input
	INT16	tblidx;					// lookup table index

	if( speed > 127 )
		speed = 127;
	else if (speed < -127 )
		speed = -127;

	// force speed positive for lookup

	if( speed < 0 )
	{
		sign = -1;
		speed *= -1;		
	}
	else
	{
		sign = 1;
	}

	// Vex regular (speed) motors do not produce outputs that are linear with inputs.
	// Use a lookup table to adjust the speed sent to the motors so linearity is achieved.

	// these steps linearize the output to input
	// use lookup tbl for base, then interpolate

	lsb = speed & 7;
	tblidx = speed >> 3;
	speed = linearSpeedTbl[tblidx] + ( ( linearSpeedTbl[tblidx+1] - linearSpeedTbl[tblidx] ) * lsb / 8 );

	speed *= sign;			// recover sign

	// compute necessary duty cycle for desired speed.  dc on duration should be .8 to 2.4 m
	// 1.6ms (duty cycle .08 for 20ms cycle) needed for motor stop
	// .8 ms either way for full fwd/rev corresponds to .04 duty cycle

	dcVal = PR2 * (.08 + ( .04 * speed / 127 ) );

	rc = PWMWrite( handle, (UINT16)dcVal );			// set duty cycle

	return(rc);
}

INT16 VexServerMotorWrite(INT16 handle, INT16 posn)
{
	double	dcVal;					// duty cycle value to output
	INT16	rc;						// retn code

	if( posn > 127 )
		posn = 127;
	else if (posn < -127 )
		posn = -127;

	// Vex servo motors are linear with inputs, so no "linearization" step is necessary.

	// compute necessary duty cycle for desired posn.  dc on duration should be .8 to 2.4 m
	// 1.6ms (duty cycle .08 for 20ms cycle) needed for motor mid-position
	// .8 ms either way for full end-to-end corresponds to .04 duty cycle

	dcVal = PR2 * (.08 + ( .04 * posn / 127 ) );

	rc = PWMWrite( handle, (UINT16)dcVal );			// set duty cycle

	return(rc);
}

INT16 VexMotorClose( INT16 handle )
{
	PWMClose( handle );
	return(PWM_SUCCESS);
}
